// project home: cp3-cg.googlecode.com

#include "cg.h"
#include <iostream>
#include "myvectormath.h"
 using namespace std;

 int cg(Int_t N, vector<Double_t> *x, vector<Double_t> *b, void (*matvec)(Int_t n, vector<Double_t> *u, vector<Double_t> *v), Double_t fTol, Int_t fMaxIter, bool fstartx0, int debug){
  if (debug>=1){cout << endl << "start debug output of cg(...)" << endl;}
  if ((unsigned)N!=x->size()){cout << "dimensions wrong!" << endl; return 1;}
  
  vector<Double_t> r(N); // restvektor
  vector<Double_t> p(N); // Verschiebungsvektor
  vector<Double_t> s(N); // product of A and p
  vector<Double_t> temp(N); // multi purpose temporay vector
  Double_t alpha;
  Double_t beta;
  Double_t rnormnew;
  
  if (fstartx0){
    x->assign(N,0); r=*b;
  }else{
//     s=matrixvectorproduct(A,JA,IA,x);
    matvec(N,x,&s);
    if (debug>=2){ // print s
      cout << "start s: ";
      for (auto it = s.cbegin(); it != s.cend(); ++it)
	cout << ' ' << *it;
      cout << '\n';
    }
    vectorsub(b,&s,&r);
    rnormnew=skalarp(&r, &r);
    if (rnormnew<fTol){
      return 0; //x is the result vector
    }
  }
  p=r;
  Double_t rnormold;
  
  Int_t fItercount=1;
  
  rnormnew=skalarp(&r, &r);
  if (debug>=1){cout << "start r: " << rnormnew << endl;}
  
  while (fItercount<=fMaxIter){
    matvec(N,&p,&s);
//     s=matrixvectorproduct(A,JA,IA,&p);
    if (debug>=2){ //print s
      cout << "s: ";
      for (auto it = s.cbegin(); it != s.cend(); ++it)
	std::cout << ' ' << *it;
      std::cout << '\n';
    }
    alpha=skalarp(&p,&r)/skalarp(&p,&s);
    if (debug>=1){cout << "alpha: " << alpha << endl;}
    
    VectorMultiS(&p,alpha,&temp);
    vectoradd(x,&temp,x);
    
    
    if (debug>=3){ // print r
    cout << "r: ";
    for (auto it = r.cbegin(); it != r.cend(); ++it)
      std::cout << ' ' << *it;
    std::cout << '\n';
    }
    VectorMultiS(&s,alpha,&temp);
    if (debug>=3){ // print temp
    cout << "temp: ";
    for (auto it = temp.cbegin(); it != temp.cend(); ++it)
      std::cout << ' ' << *it;
    std::cout << '\n';
    }
    vectorsub(&r,&temp,&r);
    
    if (debug>=3){ // print r
    cout << "r: ";
    for (auto it = r.cbegin(); it != r.cend(); ++it)
      std::cout << ' ' << *it;
    std::cout << '\n';
    }
    
    rnormold=rnormnew;
    rnormnew=skalarp(&r, &r);
    if (debug>=1){cout << fItercount << " r-norm: " << rnormnew << endl;}
    if (rnormnew<fTol){
      return 0; //x is the result vector
    }
    beta=rnormnew/rnormold;
    if (debug>=1){cout << "beta: " << beta << endl;}
    VectorMultiS(&p,beta,&temp);
    vectoradd(&r,&temp,&p);
    fItercount++;
    if (debug>=1){cout << endl;}
  }
  cout << "ERROR: MaxIterations exceeded." << endl;
  
  return 1;
}





















